#!/bin/bash
# Source ROS and catkin workspace
source /opt/ros/melodic/setup.bash
source "$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )"/devel/setup.bash

# Get the directory of this script
SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )"
BAG_DIR="$SCRIPT_DIR/BagData"

# Check if ROS is running
if ! rostopic list &>/dev/null; then
    echo "ERROR: roscore is not running. Please start ROS first."
    exit 1
fi

# 列出所有bag文件
echo "可用的bag文件:"
i=1
for bag in "$BAG_DIR"/*.bag; do
    echo "$i. $(basename "$bag")"
    bag_files[i]=$bag
    ((i++))
done

# 让用户选择bag文件
echo -n "请输入要选择的bag文件编号 (1-$((i-1))): "
read selection

# 验证输入
if [[ ! "$selection" =~ ^[0-9]+$ ]] || (( selection < 1 || selection >= i )); then
    echo "无效的选择，请输入1到$((i-1))之间的数字。"
    exit 1
fi

selected_bag="${bag_files[selection]}"

# 定义已知数据包的真实距离
declare -A true_distances=(
    ["1"]=2.4
    ["3"]=4.8
    ["4"]=8.0
    ["5"]=16.0
)

# 询问要运行的功能
echo "请选择功能:"
echo "1. 运行节点"
echo "2. 显示偏差比较"
echo -n "请输入选择 (1-2): "
read function_selection

case $function_selection in
    1)
        # 询问要运行的节点
        echo "可用的节点:"
        echo "1. odometry"
        echo "2. display_velocity"
        echo "3. display_opencv" 
        echo "4. display_pcl"
        echo "5. 全部节点"
        echo -n "请选择要运行的节点 (1-5): "
        read node_selection

        case $node_selection in
            1) 
                node="odometry"
                echo "正在运行 $node 节点，播放bag文件: $(basename "$selected_bag")"
                rosrun odometry $node &
                rosbag play "$selected_bag"
                ;;
            2) 
                node="display_velocity"
                echo "正在运行 $node 节点，播放bag文件: $(basename "$selected_bag")"
                rosrun task1-1 $node &
                rosbag play "$selected_bag"
                ;;
            3) 
                node="display_opencv"
                echo "正在运行 $node 节点，播放bag文件: $(basename "$selected_bag")"
                rosrun task1-1 $node &
                rosbag play "$selected_bag"
                ;;
            4) 
                node="display_pcl"
                echo "正在运行 $node 节点，播放bag文件: $(basename "$selected_bag")"
                rosrun task1-1 $node &
                rosbag play "$selected_bag"
                ;;
            5)
                echo "正在运行所有节点，播放bag文件: $(basename "$selected_bag")"
                rosrun odometry odometry &
                rosrun task1-1 display_velocity &
                rosrun task1-1 display_opencv &
                rosrun task1-1 display_pcl &
                rosbag play "$selected_bag"
                ;;
            *)
                echo "无效的节点选择"
                exit 1
                ;;
        esac
        ;;
    2)
        # 显示所有已知数据包的偏差比较
        echo "正在计算所有已知数据包的偏差..."
        echo ""
        echo "数据包  真实距离  方法        计算距离    偏差      偏差百分比"
        echo "--------------------------------------------------------------"
        
        # 创建序号到文件名的映射
        declare -A bag_map
        i=1
        for bag in "$BAG_DIR"/*.bag; do
            bag_map[$i]=$(basename "$bag")
            ((i++))
        done

        # 检查bc计算工具是否可用
        if ! command -v bc &> /dev/null; then
            echo "错误: bc计算工具未安装，请先安装bc"
            exit 1
        fi
        
        # 启动里程计节点并重定向输出到临时文件
        temp_file=$(mktemp)
        rosrun odometry odometry > "$temp_file" 2>&1 &
        odom_pid=$!
        
        # 等待节点启动
        sleep 2
        
        # 按顺序处理有真实值的数据包
        for bag_num in 1 3 4 5; do
            if [[ -z "${bag_map[$bag_num]}" ]]; then
                continue
            fi
            
            bag_file="$BAG_DIR/${bag_map[$bag_num]}"
            true_distance=${true_distances[$bag_num]}
            
            echo "正在处理数据包: ${bag_map[$bag_num]}"
            
            # 启动新的里程计节点并重定向输出到临时文件
            temp_file=$(mktemp)
            rosrun odometry odometry > "$temp_file" 2>&1 &
            odom_pid=$!
            
            # 等待节点启动
            sleep 2
            
            # 播放当前数据包
            rosbag play "$bag_file" > /dev/null 2>&1 &
            bag_pid=$!
            
            # 等待数据包播放完成
            while ps -p $bag_pid > /dev/null; do
                sleep 0.5
            done
            
            # 等待第一个超时警告出现
            echo "等待里程计节点超时警告..."
            while ! grep -q "数据接收超时" "$temp_file"; do
                sleep 0.5
            done
            
            # 获取当前数据包计算结果（精确匹配超时警告行）
            geo_distance=$(grep -m 1 -oP "\[WARN\] \[.*\]: 数据接收超时，几何法里程 = \K\d+\.\d+" "$temp_file")
            speed_distance=$(grep -m 1 -oP "\[WARN\] \[.*\]: 数据接收超时，几何法里程 = \d+\.\d+ m, 速度法里程 = \K\d+\.\d+" "$temp_file")
            
            # 关闭里程计节点
            if ps -p $odom_pid > /dev/null; then
                kill -SIGINT $odom_pid
                wait $odom_pid 2>/dev/null
            fi
            # 保留最近3个临时文件用于调试
            echo "临时文件已保留: $temp_file"
            # 清理旧的临时文件，保留最近3个
            ls -t /tmp/tmp.* 2>/dev/null | tail -n +4 | xargs rm -f --
            
            # 立即输出当前数据包结果
            printf "%-4s    %-8s  %-10s %-10s %-10s %-10s\n" \
                   "$bag_num" "$true_distance" "几何法" "$geo_distance" \
                   "$(echo "$geo_distance - $true_distance" | bc)" \
                   "$(echo "scale=2; ($geo_distance - $true_distance)/$true_distance*100" | bc)%"
            
            printf "%-4s    %-8s  %-10s %-10s %-10s %-10s\n" \
                   "" "" "速度法" "$speed_distance" \
                   "$(echo "$speed_distance - $true_distance" | bc)" \
                   "$(echo "scale=2; ($speed_distance - $true_distance)/$true_distance*100" | bc)%"
            
            echo "--------------------------------------------------------------"
        done
        ;;
esac

wait
